#pragma once
#include <opencv2/opencv.hpp>

#include <opencv2/highgui/highgui.hpp>
#include "pose2d.h"

class cvCanvas
{
public:
    // 画布参数
    struct cvCanvasParam
    {
        float width;      // [m] 画布宽度，物理长度m
        float height;     // [m] 画布高度
        float resolution; // [m] 分辨率，每隔像素代表多少米
        float minx;       // [m] 左下角的物理坐标x
        float miny;       // [m]左下角的物理坐标y
        float grid_width; // [m] 绘制方格子，每个格子的长度
                          // 默认参数
        cvCanvasParam()
        {
            resolution = 0.01f;
            minx = -10.f;
            miny = -10.f;
            width = 20.f;  // 2000 * 0.01f;
            height = 20.f; // 2000 * 0.01f;
            grid_width = 0.5f;
        }
    };

public:
    cvCanvas(cvCanvasParam &param);
    cvCanvas();
    // 保存为图片，better in format as "pic.png"
    void SaveToPic(std::string picName)
    {
        cv::imwrite(picName, bg);
    }
    // 绘制文字, pose 位置[m]，注意是物理坐标位置；color 颜色；thickness 线宽
    void AddText(Pose2D pose, std::string str, cv::Scalar color = cv::Scalar(0, 0, 0), int thickness = 1)
    {
        auto pos = realToIndex(pose.X, pose.Y);
        cv::putText(bg, str, pos, cv::FONT_HERSHEY_PLAIN, 1.2, color, thickness, 8, 0);
    }
    // 绘制一个点, pose:点的物理坐标
    void AddPoint(Pose2D pose, cv::Scalar color = cv::Scalar(0, 0, 0), int thickness = 1)
    {
        AddLine(pose, pose, color, thickness);
    }
    // 绘制一条直线，pose_one起点物理坐标，pose_two终点的物理坐标
    void AddLine(Pose2D pose_one, Pose2D pose_two, cv::Scalar color = cv::Scalar(0, 0, 0), int thickness = 1)
    {
        auto pos_one = realToIndex(pose_one.X, pose_one.Y);
        auto pos_two = realToIndex(pose_two.X, pose_two.Y);
        cv::line(bg, pos_one, pos_two,
                 color,
                 thickness);
    }
    // 绘制一个多边形，Poses多边形顶点的物理坐标,起点和终点不重合
    void AddPolygon(std::vector<Pose2D> Poses, cv::Scalar color = cv::Scalar(0, 0, 0), int thickness = 1)
    {
        for (size_t i = 0; i < Poses.size(); i++)
        {
            AddLine(Poses[i], Poses[(i + 1) % (Poses.size())], color, thickness);
        }
    }
    // 绘制一条路径，poses路径点的物理坐标
    void AddPath(std::vector<Pose2D> Poses, cv::Scalar color = cv::Scalar(0, 0, 0), int thickness = 1)
    {
        for (size_t i = 0; i < Poses.size() - 1; i++)
        {
            AddLine(Poses[i], Poses[(i + 1) % (Poses.size())], color, thickness);
        }
    }
    // 绘制坐标系, arrow_len 为箭头长度[m]
    void AddPoseFrame(Pose2D frame, float arrow_len = 0.6f, int thickness = 1)
    {

        auto func_drawArrow = [](cv::Mat &img, cv::Point pStart, cv::Point pEnd, int len, int alpha, cv::Scalar color, int thickness) -> void
        {
            const double PI = 3.1415926;
            cv::Point arrow;
            // 计算 θ 角（最简单的一种情况在下面图示中已经展示，关键在于 atan2 函数，详情见下面）
            double angle = atan2((double)(pStart.y - pEnd.y), (double)(pStart.x - pEnd.x));

            cv::line(img, pStart, pEnd, color, thickness);

            // 计算箭角边的另一端的端点位置（上面的还是下面的要看箭头的指向，也就是pStart和pEnd的位置）
            arrow.x = pEnd.x + len * cos(angle + PI * alpha / 180);

            arrow.y = pEnd.y + len * sin(angle + PI * alpha / 180);

            cv::line(img, pEnd, arrow, color, thickness);

            arrow.x = pEnd.x + len * cos(angle - PI * alpha / 180);

            arrow.y = pEnd.y + len * sin(angle - PI * alpha / 180);

            cv::line(img, pEnd, arrow, color, thickness);
        };

        Pose2D x_end = frame + Pose2D(arrow_len, 0, 0);
        Pose2D y_end = frame + Pose2D(0, arrow_len, 0);
        func_drawArrow(bg, realToIndex(frame.X, frame.Y), realToIndex(x_end.X, x_end.Y), 0.2 * arrow_len / mParam.resolution, 30, cv::Scalar(0, 0, 255), thickness);
        func_drawArrow(bg, realToIndex(frame.X, frame.Y), realToIndex(y_end.X, y_end.Y), 0.2 * arrow_len / mParam.resolution, 30, cv::Scalar(0, 255, 0), thickness);
    }
    void Reset();
private:
    void Reset_();
    // 绘制方格子
    void DrawGrid();
    cv::Point2i realToIndex(float x, float y);
    int x2index(float x);
    int y2index(float y);
    bool xInRange(float x)
    {
        return x >= mParam.minx && x <= mParam.minx + mParam.width;
    }
    bool yInRange(float y)
    {
        return y >= mParam.miny && y <= mParam.miny + mParam.height;
    }

protected:
    cv::Mat bg;           // 画布图片
    cvCanvasParam mParam; // 画布参数
    int mWidth;           // 画布的像素宽度
    int mHeight;          // 画布的像素高度
};
